function ENU = XYZ2ENU(XYZ, lat0, lon0)
% XYZ 为ECEF坐标系中坐标
R0 = 6378137;
XYZ0 = [R0*cos(lat0)*cos(lon0); R0*cos(lat0)*sin(lon0); R0*sin(lat0)];
R = [-sin(lon0), cos(lon0), 0; 
     -sin(lat0)*cos(lon0), -sin(lat0)*sin(lon0), cos(lat0);
     cos(lat0)*cos(lon0), cos(lat0)*sin(lon0), sin(lat0)];
ENU = R*(XYZ - XYZ0);
end